以下是我尝试读取机器人位姿的代码：
#include "robot_move_test/current_endeffector_pose_publisher.h" CurrentEndeffectorPosePublisher::CurrentEndeffectorPosePublisher(const rclcpp::NodeOptions &options) : Node("CurrentEndeffectorPosePublisher", options) { RCLCPP_INFO(this->get_logger(), "CurrentEndeffectorPosePublisher node started."); // Create a publisher for the end-effector pose pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("endeffector_pose", 10); // Create a timer to periodically publish the pose timer_ = this->create_wall_timer( std::chrono::milliseconds(1200), // Publish every 1.2 seconds std::bind(&CurrentEndeffectorPosePublisher::publishPose, this)); } void CurrentEndeffectorPosePublisher::initializeMoveGroup() { RCLCPP_INFO(this->get_logger(), "Initializing MoveGroupInterface..."); try { // Initialize MoveGroupInterface move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "panda_arm"); RCLCPP_INFO(this->get_logger(), "MoveGroupInterface initialized."); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_logger(), "Exception during MoveGroupInterface initialization: %s", e.what()); } catch (...) { RCLCPP_ERROR(this->get_logger(), "Unknown exception during MoveGroupInterface initialization."); } } void CurrentEndeffectorPosePublisher::publishPose() { if (!move_group_) { RCLCPP_WARN(this->get_logger(), "MoveGroupInterface not initialized yet!"); return; } try { auto current_pose = move_group_->getCurrentPose(); pose_publisher_->publish(current_pose); RCLCPP_INFO(this->get_logger(), "Current Endeffector Pose: pos_x=%.3f, pos_y=%.3f, pos_z=%.3f, quat_x=%.3f, quat_y=%.3f, quat_z=%.3f, quat_w=%.3f", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z, current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_logger(), "Exception during publishPose: %s", e.what()); } catch (...) { RCLCPP_ERROR(this->get_logger(), "Unknown exception during publishPose."); } } int main(int argc, char **argv) { rclcpp::init(argc, argv); try { auto node = std::make_shared<CurrentEndeffectorPosePublisher>(rclcpp::NodeOptions()); node->initializeMoveGroup(); // Initialize MoveGroupInterface after the node is fully constructed rclcpp::spin(node); } catch (const std::exception &e) { std::cerr << "Exception in main: " << e.what() << std::endl; } catch (...) { std::cerr << "Unknown exception in main." << std::endl; } rclcpp::shutdown(); return 0; }